function fval=InvKinematics(X,L)
%Define six variables
theta1z=X(1);
theta2x=X(2);
theta3x=X(3);
theta4p=X(4);
theta5z=X(5);
theta6x=X(6);

%Define f(X)
P0=[T(0,0,0) [0;0;0];0 0 0 1];
T01=[T(0,0,0) [0;0;10];0 0 0 1];
T12=[T(0,0,theta1z) [0;0;0];0 0 0 1]*[T(0,0,0) [2;0;0];0 0 0 1];
T23=[T(theta2x,0,0) [0;0;0];0 0 0 1]*[T(0,0,0) [0;0;10];0 0 0 1];
T34=[T(theta3x,0,0) [0;0;0];0 0 0 1]*[T(0,0,0) [-2;0;0];0 0 0 1];
T45=[T(0,0,0) [0;0;theta4p];0 0 0 1];
T56=[T(0,0,theta5z) [0;0;0];0 0 0 1]*[T(0,0,0) [0;0;10];0 0 0 1];
T67=[T(theta6x,0,0) [0;0;0];0 0 0 1]*[T(0,0,0) [0;0;10];0 0 0 1];
T07=T01*T12*T23*T34*T45*T56*T67;
P07=P0*T07;
P07=P07-L;

fval=P07(1:3,1:4);

